/*
 * @brief: motor cam driver
 * @date: 2015.12.12
 */
#include "motor_cam.h"
#include "mhdef.h"
#include "dispatch.h"
#include <assert.h>
#include <string.h>
#include <stdio.h>

void motor_cam_setup_reset_msg(uchar *data, short *find_zero, int num)
{
    int i = 0;

    for (i = 0; i < num; i++)
    {
        ushort cmd = 0;
        if (find_zero[i])
        {
            cmd = 0xA000;
        }

        data[i * 2] = cmd & 0x00FF;
        data[i * 2 + 1] = (cmd & 0xFF00) >> 8;
    }
}

void motor_cam_setup_exec_msg(uchar *data, short *delta, short *find_zero, int num)
{
    int i = 0;

    for (i = 0; i < num; i++)
    {
        ushort cmd = 0;
        if (delta[i] > 0)
        {
            cmd = 0x4000 | delta[i];
        }
        else
        {
            cmd = 0x7000 | -delta[i];
            if (find_zero[i])
            {
                cmd = 0x8000 | -delta[i];
            }
        }

        data[i * 2] = cmd & 0x00FF;
        data[i * 2 + 1] = (cmd & 0xFF00) >> 8;
    }
}

int motor_cam_exec(MH_UDP *pudp, int ctype, short *delta, short *find_zero)
{
    assert(delta != NULL);
    assert(find_zero != NULL);

    CAN_MSG msg = { 0, 0, { 0 } };
    int ret = 0;

    msg.id = ID_MOTOR_CAM;
    msg.len = 8;

    memset(msg.data, 0, 8);
    motor_cam_setup_exec_msg(msg.data, delta, find_zero, 4);

    printf("msg.id = %x\n", msg.id);
    printf("msg.data = %x, %x, %x, %x, %x, %x, %x, %x\n",
        msg.data[0], msg.data[1], msg.data[2], msg.data[3], \
        msg.data[4], msg.data[5], msg.data[6], msg.data[7]);
    ret = dispatch_msg(pudp, &msg, NULL);

    return ret;
}

int motor_cam_reset(MH_UDP *pudp, int ctype, short *find_zero)
{
    assert(find_zero != NULL);

    CAN_MSG msg = { 0, 0, { 0 } };
    int ret = 0;

    /* 第一次消息 */
    msg.id = ID_MOTOR_CAM;
    msg.len = 8;

    memset(msg.data, 0, 8);

    motor_cam_setup_reset_msg(msg.data, find_zero, 4);

    printf("msg.id = %x\n", msg.id);
    printf("msg.data = %x, %x, %x, %x, %x, %x, %x, %x\n",
        msg.data[0], msg.data[1], msg.data[2], msg.data[3], \
        msg.data[4], msg.data[5], msg.data[6], msg.data[7]);
    ret = dispatch_msg(pudp,&msg, NULL);

    return ret;
}

int motor_cam_set_zero_delta(MH_UDP *pudp, char *delta)
{
    CAN_MSG msg = { 0 , 0, { 0 } };

    msg.id = ID_CAM_MOTOR_DELTA;
    msg.len = 8;
    msg.data[0] = 0;
    msg.data[1] = 0;
    msg.data[2] = delta[0];
    msg.data[3] = delta[1];
    msg.data[4] = delta[2];
    msg.data[5] = delta[3];
    msg.data[6] = 0;
    msg.data[7] = 0;

    return dispatch_msg(pudp, &msg, NULL);
}

